clc;
clear;
par.samplePeriod = 0.02;
par.g = 9.7934752114984;
par.C = [0 0 0];

% 生成轨迹
% 常量
cst.sec2Rad = pi/3600/180;
cst.deg2Rad = pi/180;
cst_trajGen = load('earthconstants_WGS84.mat', 'omegaIE', 'RE', 'eSq', 'f');

% 配置
cfg_trajGen.enableTrajGen = true;
cfg_trajGen.enableNavigation = false;
cfg_trajGen.IMU.enableErr = true;
cfg_trajGen.IMU.enableNoise = true;
cfg_trajGen.IMU.ignore2OdErr = false;
cfg_trajGen.IMU.quantizeOut = false;
cfg_trajGen.IMU.outFilePath = [];
if cfg_trajGen.IMU.quantizeOut
    cfg_trajGen.IMU.outPrecision = [];
else
    cfg_trajGen.IMU.outPrecision = '%.16e';
end

% 输入
in_trajGen.genFuncName = 'gentgi_navtest_regular';
in_trajGen.genFuncInArg = {};

% 参量
par_trajGen.samplePeriod = par.samplePeriod;
par_trajGen.g = par.g;
par_trajGen.IMU.acc.B = [33 33 33]' * par_trajGen.g * 1e-6; % 转换前单位μg，转换后单位m/s^2
par_trajGen.IMU.acc.SF0 = ones(3, 1);
par_trajGen.IMU.gyro.SF0 = ones(3, 1);

% 初始条件
iniCon_trajGen.CVG = eye(3); % 初始姿态矩阵
iniCon_trajGen.CBV = eye(3);
iniCon_trajGen.vG = zeros(3, 1); % 初始速度
iniCon_trajGen.pos = [30*cst.deg2Rad, 110*cst.deg2Rad, 0]'; % 纬度、经度、高度

% 设置当前目录为轨迹发生器函数所在目录
fileID = fopen('gentrajectory.m');
fileFullPath = fopen(fileID);
fclose(fileID);
folderPath = getdirandfilenamefrompath(fileFullPath);
cd(folderPath);
% 运行轨迹仿真
[out_trajGen, auxOut_trajGen] = gentrajectory(in_trajGen, cfg_trajGen, par_trajGen, iniCon_trajGen, cst_trajGen);

% 提取参考值
ref.t = out_trajGen.t;
ref.pos = out_trajGen.pos(:, :, 1);
ref.vG = out_trajGen.vG(:, :, 1);
ref.CBG = out_trajGen.CBG;

%% 正常导航
% 常量
cst_SINI = load('earthconstants_WGS84.mat', 'RE', 'f', 'mu', 'J2', 'J3', 'omegaIE');

% 配置
cfg_SINI.NSInLS = 1; % 低速解算周期内的中速解算周期数
cfg_SINI.HSInNS = 1; % 中速解算周期内的高速解算周期数
cfg_SINI.useDCMInAttCalc = true;
cfg_SINI.useNormOrthoInAttCalc = true; % true表示在姿态解算中使用规范化（及正交化），false表示不使用
cfg_SINI.useHiResPosNSCal = false;
cfg_SINI.useExtGravity = true; % true表示在使用外部提供的重力加速度，false表示使用重力模型计算重力
cfg_SINI.navCoordType = NavCoordType.GEO;

% 参量
par_SINI.Tl = par.samplePeriod;
par_SINI.C = par.C;

in.accOut = out_trajGen.IMU.accOutErrFree;
in.gyroOut = out_trajGen.IMU.gyroOutErrFree;
% in.accOut = out_trajGen.IMU.accOut;
% in.gyroOut = out_trajGen.IMU.gyroOut;

% 初始条件
iniCon_SINI.L_n = out_trajGen.pos(1, 1);
iniCon_SINI.L_n1 = iniCon_SINI.L_n;
iniCon_SINI.lambda_n = out_trajGen.pos(1, 2);
iniCon_SINI.lambda_n1 = iniCon_SINI.lambda_n;
iniCon_SINI.h_n = out_trajGen.pos(1, 3);
iniCon_SINI.h_n1 = iniCon_SINI.h_n;
iniCon_SINI.vN_m = out_trajGen.vG(1, 1:3)';
iniCon_SINI.vN_m1 = iniCon_SINI.vN_m;
iniCon_SINI.vN_n1 = iniCon_SINI.vN_m;
iniCon_SINI.CBL = [0 1 0; 1 0 0; 0 0 -1]*out_trajGen.CBG(:, :, 1);
iniCon_SINI.specVelInc_lx = in.accOut(1, :)'; % 0时刻之前的比速度增量
iniCon_SINI.angleInc_lx = in.gyroOut(1, :)';

% 运行
out_SINI.cnt = NaN(length(in.accOut), 4);
out_SINI.CNE = NaN(3, 3, length(in.accOut));
out_SINI.pos = NaN(length(in.accOut), 3);
out_SINI.vN = NaN(length(in.accOut), 3);
out_SINI.vG = NaN(length(in.accOut), 3);
out_SINI.CBL = NaN(3, 3, length(in.accOut));
out_SINI.CBG = NaN(3, 3, length(in.accOut));
out_SINI.qBL = NaN(length(in.accOut), 4);
out_SINI.att = NaN(length(in.accOut), 3);
out_SINI.psiTrue = NaN(length(in.accOut), 1);
out_SINI.alpha = NaN(length(in.accOut), 1);
out_SINI.gN = NaN(length(in.accOut), 3);
out_SINI.FCN = NaN(3, 3, length(in.accOut));
auxOut_SINI.altErr = NaN(length(in.accOut), 1);
auxOut_SINI.altErrIntGain = NaN(length(in.accOut), 1);
auxOut_SINI.coning = NaN(length(in.accOut), 3);
auxOut_SINI.sculling = NaN(length(in.accOut), 3);
auxOut_SINI.scrolling = NaN(length(in.accOut), 3);

clear('sinistep_pgs');
hwb = waitbar(0, '开始解算...');
for i=1:length(in.accOut) % i=1时初始化，i>=1时为解算周期
    waitbar(double(i)/double(length(in.accOut)), hwb, ...
        ['导航解算中...完成 ',int2str(100*double(i)/double(length(in.accOut))),' %']);
    if i==1
        in_SINI = struct('specVelInc', in.accOut(i, :)', 'angleInc', in.gyroOut(i, :)',...
            'g', par.g,  'hAltm', NaN, 'CNEExt', NaN(3), 'hExt', NaN,...
            'vNExt', NaN(3, 1), 'CBLExt', NaN(3), 'qBLExt', NaN(1, 4), ...
            'beta_m_ext', NaN(3,1), 'DvScul_m_ext', NaN(3,1), 'DrScrl_m_ext', NaN(3,1));
        mid_t = double(i-1)*par_SINI.Tl;
        [tmp.out, tmp.auxOut] = sinistep_pgs(int8(0), in_SINI, cfg_SINI, par_SINI, iniCon_SINI, cst_SINI);
        out_SINI.t(i,:) = mid_t;
        out_SINI.cnt(i, :) = [tmp.out.cycCnt, tmp.out.HSCnt, tmp.out.NSCnt, tmp.out.LSCnt];
        out_SINI.CNE(:, :, i) = tmp.out.CNE;
        out_SINI.pos(i, :) = [tmp.out.L, tmp.out.lambda, tmp.out.h];
        out_SINI.vN(i, :) = tmp.out.vN';
        out_SINI.vG(i, :) = tmp.out.vG';
        out_SINI.CBL(:, :, i) = tmp.out.CBL;
        out_SINI.CBG(:, :, i) = tmp.out.CBG;
        out_SINI.qBL(i, :) = tmp.out.qBL;
        out_SINI.att(i, :) = [tmp.out.psi, tmp.out.theta, tmp.out.phi];
        out_SINI.psiTrue(i) = tmp.out.psiTrue;
        out_SINI.alpha(i) = tmp.out.alpha;
        out_SINI.gN(i, :) = tmp.out.gN';
        out_SINI.FCN(:, :, i) = tmp.out.FCN;
        auxOut_SINI.altErr(i) = tmp.auxOut.delh_n;
        auxOut_SINI.altErrIntGain(i) = tmp.auxOut.eVC3_n;
        auxOut_SINI.coning(i, :) = tmp.auxOut.beta_l';
        auxOut_SINI.sculling(i, :) = tmp.auxOut.DvScul_l';
        auxOut_SINI.scrolling(i, :) = tmp.auxOut.DrScrl_l';
    end
    in_SINI = struct('specVelInc', in.accOut(i, :)', 'angleInc', in.gyroOut(i, :)',...
        'g', par.g,  'hAltm', NaN, 'CNEExt', NaN(3), 'hExt', NaN,...
        'vNExt', NaN(3, 1), 'CBLExt', NaN(3), 'qBLExt', NaN(1, 4), ...
        'beta_m_ext', NaN(3,1), 'DvScul_m_ext', NaN(3,1), 'DrScrl_m_ext', NaN(3,1));
    mid_t = double(i)*par_SINI.Tl;
    [tmp.out, tmp.auxOut] = sinistep_pgs(int8(1), in_SINI, cfg_SINI, par_SINI, iniCon_SINI, cst_SINI);
    out_SINI.t(i+1,:) = mid_t;
    out_SINI.cnt(i+1, :) = [tmp.out.cycCnt, tmp.out.HSCnt, tmp.out.NSCnt, tmp.out.LSCnt];
    out_SINI.CNE(:, :, i+1) = tmp.out.CNE;
    out_SINI.pos(i+1, :) = [tmp.out.L, tmp.out.lambda, tmp.out.h];
    out_SINI.vN(i+1, :) = tmp.out.vN';
    out_SINI.vG(i+1, :) = tmp.out.vG';
    out_SINI.CBL(:, :, i+1) = tmp.out.CBL;
    out_SINI.CBG(:, :, i+1) = tmp.out.CBG;
    out_SINI.qBL(i+1, :) = tmp.out.qBL;
    out_SINI.att(i+1, :) = [tmp.out.psi, tmp.out.theta, tmp.out.phi];
    out_SINI.psiTrue(i+1) = tmp.out.psiTrue;
    out_SINI.alpha(i+1) = tmp.out.alpha;
    out_SINI.gN(i+1, :) = tmp.out.gN';
    out_SINI.FCN(:, :, i+1) = tmp.out.FCN;
    auxOut_SINI.altErr(i+1) = tmp.auxOut.delh_n;
    auxOut_SINI.altErrIntGain(i+1) = tmp.auxOut.eVC3_n;
    auxOut_SINI.coning(i+1, :) = tmp.auxOut.beta_l';
    auxOut_SINI.sculling(i+1, :) = tmp.auxOut.DvScul_l';
    auxOut_SINI.scrolling(i+1, :) = tmp.auxOut.DrScrl_l';
end
close(hwb);
% 显示验证结果
disp('===================正常导航===================');
out_sim = comparenavparam({out_SINI}, ref, cst_SINI.RE, {1, 2, 3}, {'导航'}, '轨迹');
disp('最大位置误差（m）');
disp(max(abs(out_sim.dPos{1}), [], 1));
disp('最大速度误差（m/s）');
disp(max(abs(out_sim.dvG{1}), [], 1));
disp('最大姿态误差（″）');
disp(max(abs(out_sim.phi{1}), [], 1));
disp('最大正交误差（″）');
disp(max(abs(out_sim.dOrth{1}), [], 1));
disp('最大规范误差（ppm）');
disp(max(abs(out_sim.dNorm{1}), [], 1));

%% 逆向导航 -- 接着上述正向导航进行
out_RSINI.cnt = NaN(length(in.accOut), 4);
out_RSINI.CNE = NaN(3, 3, length(in.accOut));
out_RSINI.pos = NaN(length(in.accOut), 3);
out_RSINI.vN = NaN(length(in.accOut), 3);
out_RSINI.vG = NaN(length(in.accOut), 3);
out_RSINI.CBL = NaN(3, 3, length(in.accOut));
out_RSINI.CBG = NaN(3, 3, length(in.accOut));
out_RSINI.qBL = NaN(length(in.accOut), 4);
out_RSINI.att = NaN(length(in.accOut), 3);
out_RSINI.psiTrue = NaN(length(in.accOut), 1);
out_RSINI.alpha = NaN(length(in.accOut), 1);
out_RSINI.gN = NaN(length(in.accOut), 3);
out_RSINI.FCN = NaN(3, 3, length(in.accOut));
auxOut_RSINI.altErr = NaN(length(in.accOut), 1);
auxOut_RSINI.altErrIntGain = NaN(length(in.accOut), 1);
auxOut_RSINI.coning = NaN(length(in.accOut), 3);
auxOut_RSINI.sculling = NaN(length(in.accOut), 3);
auxOut_RSINI.scrolling = NaN(length(in.accOut), 3);

cst_SINI = load('earthconstants_WGS84.mat', 'RE', 'f', 'mu', 'J2', 'J3', 'omegaIE');
cst_SINI.omegaIE = -cst_SINI.omegaIE;
% 初始条件
iniCon_SINI.L_n = out_SINI.pos(end, 1);
iniCon_SINI.L_n1 = iniCon_SINI.L_n;
iniCon_SINI.lambda_n = out_SINI.pos(end, 2);
iniCon_SINI.lambda_n1 = iniCon_SINI.lambda_n;
iniCon_SINI.h_n = out_SINI.pos(end, 3);
iniCon_SINI.h_n1 = iniCon_SINI.h_n;
iniCon_SINI.vN_m = -out_SINI.vG(end, 1:3)';
iniCon_SINI.vN_m1 = iniCon_SINI.vN_m;
iniCon_SINI.vN_n1 = iniCon_SINI.vN_m;
iniCon_SINI.CBL = out_SINI.CBL(:, :, end);
iniCon_SINI.specVelInc_lx = in.accOut(end, :)'; % 0时刻之前的比速度增量
iniCon_SINI.angleInc_lx = -in.gyroOut(end, :)';

hwb = waitbar(0, '开始逆向解算...');
for i=length(in.accOut):-1:1
    waitbar(double(i)/double(length(in.accOut)), hwb, ...
        ['逆向导航解算中...完成 ',int2str(100*double(i)/double(length(in.accOut))),' %']);
    if i==length(in.accOut)
        in_SINI = struct('specVelInc', in.accOut(i, :)', 'angleInc', -in.gyroOut(i, :)',...
            'g', par.g,  'hAltm', NaN, 'CNEExt', NaN(3), 'hExt', NaN,...
            'vNExt', NaN(3,1), 'CBLExt', NaN(3), 'qBLExt', NaN(1, 4), ...
            'beta_m_ext', NaN(3, 1), 'DvScul_m_ext', NaN(3, 1), 'DrScrl_m_ext', NaN(3, 1));
        mid_t = double(i)*par_SINI.Tl;
        [tmp.out, tmp.auxOut] = sinistep_pgs(int8(0), in_SINI, cfg_SINI, par_SINI, iniCon_SINI, cst_SINI);
        out_RSINI.t(i+1,:) = mid_t;
        out_RSINI.cnt(i+1, :) = [tmp.out.cycCnt, tmp.out.HSCnt, tmp.out.NSCnt, tmp.out.LSCnt];
        out_RSINI.CNE(:, :, i+1) = tmp.out.CNE;
        out_RSINI.pos(i+1, :) = [tmp.out.L, tmp.out.lambda, tmp.out.h];
        out_RSINI.vN(i+1, :) = -tmp.out.vN';
        out_RSINI.vG(i+1, :) = -tmp.out.vG';
        out_RSINI.CBL(:, :, i+1) = tmp.out.CBL;
        out_RSINI.CBG(:, :, i+1) = tmp.out.CBG;
        out_RSINI.qBL(i+1, :) = tmp.out.qBL;
        out_RSINI.att(i+1, :) = [tmp.out.psi, tmp.out.theta, tmp.out.phi];
        out_RSINI.psiTrue(i+1) = tmp.out.psiTrue;
        out_RSINI.alpha(i+1) = tmp.out.alpha;
        out_RSINI.gN(i+1, :) = tmp.out.gN';
        out_RSINI.FCN(:, :, i+1) = tmp.out.FCN;
        auxOut_RSINI.altErr(i+1) = tmp.auxOut.delh_n;
        auxOut_RSINI.altErrIntGain(i+1) = tmp.auxOut.eVC3_n;
        auxOut_RSINI.coning(i+1, :) = tmp.auxOut.beta_l';
        auxOut_RSINI.sculling(i+1, :) = tmp.auxOut.DvScul_l';
        auxOut_RSINI.scrolling(i+1, :) = tmp.auxOut.DrScrl_l';
    end
    in_SINI = struct('specVelInc', in.accOut(i, :)', 'angleInc', -in.gyroOut(i, :)',...
        'g', par.g,  'hAltm', NaN, 'CNEExt', NaN(3), 'hExt', NaN,...
        'vNExt', NaN(3,1), 'CBLExt', NaN(3), 'qBLExt', NaN(1, 4), ...
        'beta_m_ext', NaN(3, 1), 'DvScul_m_ext', NaN(3, 1), 'DrScrl_m_ext', NaN(3, 1));
    mid_t = double(i-1)*par_SINI.Tl;
    [tmp.out, tmp.auxOut] = sinistep_pgs(int8(1), in_SINI, cfg_SINI, par_SINI, iniCon_SINI, cst_SINI);
    out_RSINI.t(i,:) = mid_t;
    out_RSINI.cnt(i, :) = [tmp.out.cycCnt, tmp.out.HSCnt, tmp.out.NSCnt, tmp.out.LSCnt];
    out_RSINI.CNE(:, :, i) = tmp.out.CNE;
    out_RSINI.pos(i, :) = [tmp.out.L, tmp.out.lambda, tmp.out.h];
    out_RSINI.vN(i, :) = -tmp.out.vN';
    out_RSINI.vG(i, :) = -tmp.out.vG';
    out_RSINI.CBL(:, :, i) = tmp.out.CBL;
    out_RSINI.CBG(:, :, i) = tmp.out.CBG;
    out_RSINI.qBL(i, :) = tmp.out.qBL;
    out_RSINI.att(i, :) = [tmp.out.psi, tmp.out.theta, tmp.out.phi];
    out_RSINI.psiTrue(i) = tmp.out.psiTrue;
    out_RSINI.alpha(i) = tmp.out.alpha;
    out_RSINI.gN(i, :) = tmp.out.gN';
    out_RSINI.FCN(:, :, i) = tmp.out.FCN;
    auxOut_RSINI.altErr(i) = tmp.auxOut.delh_n;
    auxOut_RSINI.altErrIntGain(i) = tmp.auxOut.eVC3_n;
    auxOut_RSINI.coning(i, :) = tmp.auxOut.beta_l';
    auxOut_RSINI.sculling(i, :) = tmp.auxOut.DvScul_l';
    auxOut_RSINI.scrolling(i, :) = tmp.auxOut.DrScrl_l';
end
close(hwb);
% 显示验证结果
disp('===================逆向导航===================');
out_sim = comparenavparam({out_RSINI}, ref, cst_SINI.RE, {4, 5, 6}, {'导航'}, '轨迹');
disp('最大位置误差（m）');
disp(max(abs(out_sim.dPos{1}), [], 1));
disp('最大速度误差（m/s）');
disp(max(abs(out_sim.dvG{1}), [], 1));
disp('最大姿态误差（″）');
disp(max(abs(out_sim.phi{1}), [], 1));
disp('最大正交误差（″）');
disp(max(abs(out_sim.dOrth{1}), [], 1));
disp('最大规范误差（ppm）');
disp(max(abs(out_sim.dNorm{1}), [], 1));

% 正向逆向对比
disp('===================正向逆向导航对比===================');
out_sim = comparenavparam({out_RSINI}, out_SINI, cst_SINI.RE, {7, 8, 9}, {'逆向导航'}, '正向导航');
disp('最大位置误差（m）');
disp(max(abs(out_sim.dPos{1}), [], 1));
disp('最大速度误差（m/s）');
disp(max(abs(out_sim.dvG{1}), [], 1));
disp('最大姿态误差（″）');
disp(max(abs(out_sim.phi{1}), [], 1));
disp('最大正交误差（″）');
disp(max(abs(out_sim.dOrth{1}), [], 1));
disp('最大规范误差（ppm）');
disp(max(abs(out_sim.dNorm{1}), [], 1));

%% 正向导航 -- 接着上述逆向导航开始
out_SINI.cnt = NaN(length(in.accOut), 4);
out_SINI.CNE = NaN(3, 3, length(in.accOut));
out_SINI.pos = NaN(length(in.accOut), 3);
out_SINI.vN = NaN(length(in.accOut), 3);
out_SINI.vG = NaN(length(in.accOut), 3);
out_SINI.CBL = NaN(3, 3, length(in.accOut));
out_SINI.CBG = NaN(3, 3, length(in.accOut));
out_SINI.qBL = NaN(length(in.accOut), 4);
out_SINI.att = NaN(length(in.accOut), 3);
out_SINI.psiTrue = NaN(length(in.accOut), 1);
out_SINI.alpha = NaN(length(in.accOut), 1);
out_SINI.gN = NaN(length(in.accOut), 3);
out_SINI.FCN = NaN(3, 3, length(in.accOut));
auxOut_SINI.altErr = NaN(length(in.accOut), 1);
auxOut_SINI.altErrIntGain = NaN(length(in.accOut), 1);
auxOut_SINI.coning = NaN(length(in.accOut), 3);
auxOut_SINI.sculling = NaN(length(in.accOut), 3);
auxOut_SINI.scrolling = NaN(length(in.accOut), 3);

cst_SINI = load('earthconstants_WGS84.mat', 'RE', 'f', 'mu', 'J2', 'J3', 'omegaIE');
% 初始条件
iniCon_SINI.L_n = out_RSINI.pos(1, 1);
iniCon_SINI.L_n1 = iniCon_SINI.L_n;
iniCon_SINI.lambda_n = out_RSINI.pos(1, 2);
iniCon_SINI.lambda_n1 = iniCon_SINI.lambda_n;
iniCon_SINI.h_n = out_RSINI.pos(1, 3);
iniCon_SINI.h_n1 = iniCon_SINI.h_n;
iniCon_SINI.vN_m = -out_RSINI.vG(1, 1:3)';
iniCon_SINI.vN_m1 = iniCon_SINI.vN_m;
iniCon_SINI.vN_n1 = iniCon_SINI.vN_m;
iniCon_SINI.CBL = out_RSINI.CBL(:, :, 1);
iniCon_SINI.specVelInc_lx = in.accOut(1, :)'; % 0时刻之前的比速度增量
iniCon_SINI.angleInc_lx = in.gyroOut(1, :)';

hwb = waitbar(0, '正向开始解算...');
for i=1:length(in.accOut)
    waitbar(double(i)/double(length(in.accOut)), hwb, ...
        ['正向导航解算中...完成 ',int2str(100*double(i)/double(length(in.accOut))),' %']);
    if i==1
        in_SINI = struct('specVelInc', in.accOut(i, :)', 'angleInc', in.gyroOut(i, :)',...
            'g', par.g,  'hAltm', NaN, 'CNEExt', NaN(3), 'hExt', NaN,...
            'vNExt', NaN(3, 1), 'CBLExt', NaN(3), 'qBLExt', NaN(1, 4), ...
            'beta_m_ext', NaN(3,1), 'DvScul_m_ext', NaN(3,1), 'DrScrl_m_ext', NaN(3,1));
        mid_t = double(i-1)*par_SINI.Tl;
        [tmp.out, tmp.auxOut] = sinistep_pgs(int8(0), in_SINI, cfg_SINI, par_SINI, iniCon_SINI, cst_SINI);
        out_SINI.t(i,:) = mid_t;
        out_SINI.cnt(i, :) = [tmp.out.cycCnt, tmp.out.HSCnt, tmp.out.NSCnt, tmp.out.LSCnt];
        out_SINI.CNE(:, :, i) = tmp.out.CNE;
        out_SINI.pos(i, :) = [tmp.out.L, tmp.out.lambda, tmp.out.h];
        out_SINI.vN(i, :) = tmp.out.vN';
        out_SINI.vG(i, :) = tmp.out.vG';
        out_SINI.CBL(:, :, i) = tmp.out.CBL;
        out_SINI.CBG(:, :, i) = tmp.out.CBG;
        out_SINI.qBL(i, :) = tmp.out.qBL;
        out_SINI.att(i, :) = [tmp.out.psi, tmp.out.theta, tmp.out.phi];
        out_SINI.psiTrue(i) = tmp.out.psiTrue;
        out_SINI.alpha(i) = tmp.out.alpha;
        out_SINI.gN(i, :) = tmp.out.gN';
        out_SINI.FCN(:, :, i) = tmp.out.FCN;
        auxOut_SINI.altErr(i) = tmp.auxOut.delh_n;
        auxOut_SINI.altErrIntGain(i) = tmp.auxOut.eVC3_n;
        auxOut_SINI.coning(i, :) = tmp.auxOut.beta_l';
        auxOut_SINI.sculling(i, :) = tmp.auxOut.DvScul_l';
        auxOut_SINI.scrolling(i, :) = tmp.auxOut.DrScrl_l';
    end
    in_SINI = struct('specVelInc', in.accOut(i, :)', 'angleInc', in.gyroOut(i, :)',...
        'g', par.g,  'hAltm', NaN, 'CNEExt', NaN(3), 'hExt', NaN,...
        'vNExt', NaN(3, 1), 'CBLExt', NaN(3), 'qBLExt', NaN(1, 4), ...
        'beta_m_ext', NaN(3,1), 'DvScul_m_ext', NaN(3,1), 'DrScrl_m_ext', NaN(3,1));
    mid_t = double(i)*par_SINI.Tl;
    [tmp.out, tmp.auxOut] = sinistep_pgs(int8(1), in_SINI, cfg_SINI, par_SINI, iniCon_SINI, cst_SINI);
    out_SINI.t(i+1,:) = mid_t;
    out_SINI.cnt(i+1, :) = [tmp.out.cycCnt, tmp.out.HSCnt, tmp.out.NSCnt, tmp.out.LSCnt];
    out_SINI.CNE(:, :, i+1) = tmp.out.CNE;
    out_SINI.pos(i+1, :) = [tmp.out.L, tmp.out.lambda, tmp.out.h];
    out_SINI.vN(i+1, :) = tmp.out.vN';
    out_SINI.vG(i+1, :) = tmp.out.vG';
    out_SINI.CBL(:, :, i+1) = tmp.out.CBL;
    out_SINI.CBG(:, :, i+1) = tmp.out.CBG;
    out_SINI.qBL(i+1, :) = tmp.out.qBL;
    out_SINI.att(i+1, :) = [tmp.out.psi, tmp.out.theta, tmp.out.phi];
    out_SINI.psiTrue(i+1) = tmp.out.psiTrue;
    out_SINI.alpha(i+1) = tmp.out.alpha;
    out_SINI.gN(i+1, :) = tmp.out.gN';
    out_SINI.FCN(:, :, i+1) = tmp.out.FCN;
    auxOut_SINI.altErr(i+1) = tmp.auxOut.delh_n;
    auxOut_SINI.altErrIntGain(i+1) = tmp.auxOut.eVC3_n;
    auxOut_SINI.coning(i+1, :) = tmp.auxOut.beta_l';
    auxOut_SINI.sculling(i+1, :) = tmp.auxOut.DvScul_l';
    auxOut_SINI.scrolling(i+1, :) = tmp.auxOut.DrScrl_l';
end
close(hwb);
% 显示验证结果
disp('===================正常导航===================');
out_sim = comparenavparam({out_SINI}, ref, cst_SINI.RE, {10, 11, 12}, {'导航'}, '轨迹');
disp('最大位置误差（m）');
disp(max(abs(out_sim.dPos{1}), [], 1));
disp('最大速度误差（m/s）');
disp(max(abs(out_sim.dvG{1}), [], 1));
disp('最大姿态误差（″）');
disp(max(abs(out_sim.phi{1}), [], 1));
disp('最大正交误差（″）');
disp(max(abs(out_sim.dOrth{1}), [], 1));
disp('最大规范误差（ppm）');
disp(max(abs(out_sim.dNorm{1}), [], 1));

% 正向逆向对比
disp('===================正向逆向导航对比===================');
out_sim = comparenavparam({out_RSINI}, out_SINI, cst_SINI.RE, {13, 14, 15}, {'逆向导航'}, '正向导航');
disp('最大位置误差（m）');
disp(max(abs(out_sim.dPos{1}), [], 1));
disp('最大速度误差（m/s）');
disp(max(abs(out_sim.dvG{1}), [], 1));
disp('最大姿态误差（″）');
disp(max(abs(out_sim.phi{1}), [], 1));
disp('最大正交误差（″）');
disp(max(abs(out_sim.dOrth{1}), [], 1));
disp('最大规范误差（ppm）');
disp(max(abs(out_sim.dNorm{1}), [], 1));

%% 单独初始化逆向导航
cst_SINI = load('earthconstants_WGS84.mat', 'RE', 'f', 'mu', 'J2', 'J3', 'omegaIE');
cst_SINI.omegaIE = -cst_SINI.omegaIE;
% 初始条件
iniCon_SINI.L_n = out_trajGen.pos(end, 1);
iniCon_SINI.L_n1 = iniCon_SINI.L_n;
iniCon_SINI.lambda_n = out_trajGen.pos(end, 2);
iniCon_SINI.lambda_n1 = iniCon_SINI.lambda_n;
iniCon_SINI.h_n = out_trajGen.pos(end, 3);
iniCon_SINI.h_n1 = iniCon_SINI.h_n;
iniCon_SINI.vN_m = -out_trajGen.vG(end, 1:3)';
iniCon_SINI.vN_m1 = iniCon_SINI.vN_m;
iniCon_SINI.vN_n1 = iniCon_SINI.vN_m;
iniCon_SINI.CBL = [0 1 0; 1 0 0; 0 0 -1]*out_trajGen.CBG(:, :, end);
iniCon_SINI.specVelInc_lx = in.accOut(end, :)'; % 0时刻之前的比速度增量
iniCon_SINI.angleInc_lx = -in.gyroOut(end, :)';

% 运行
out_RSINI.cnt = NaN(length(in.accOut), 4);
out_RSINI.CNE = NaN(3, 3, length(in.accOut));
out_RSINI.pos = NaN(length(in.accOut), 3);
out_RSINI.vN = NaN(length(in.accOut), 3);
out_RSINI.vG = NaN(length(in.accOut), 3);
out_RSINI.CBL = NaN(3, 3, length(in.accOut));
out_RSINI.CBG = NaN(3, 3, length(in.accOut));
out_RSINI.qBL = NaN(length(in.accOut), 4);
out_RSINI.att = NaN(length(in.accOut), 3);
out_RSINI.psiTrue = NaN(length(in.accOut), 1);
out_RSINI.alpha = NaN(length(in.accOut), 1);
out_RSINI.gN = NaN(length(in.accOut), 3);
out_RSINI.FCN = NaN(3, 3, length(in.accOut));
auxOut_RSINI.altErr = NaN(length(in.accOut), 1);
auxOut_RSINI.altErrIntGain = NaN(length(in.accOut), 1);
auxOut_RSINI.coning = NaN(length(in.accOut), 3);
auxOut_RSINI.sculling = NaN(length(in.accOut), 3);
auxOut_RSINI.scrolling = NaN(length(in.accOut), 3);

clear('sinistep_pgs');
hwb = waitbar(0, '开始逆向解算...');
for i=length(in.accOut):-1:1 % i=1时初始化，i>=1时为解算周期
    waitbar(double(i)/double(length(in.accOut)), hwb, ...
        ['逆向导航解算中...完成 ',int2str(100*double(i)/double(length(in.accOut))),' %']);
    if i==length(in.accOut)
        in_SINI = struct('specVelInc', in.accOut(i, :)', 'angleInc', -in.gyroOut(i, :)',...
            'g', par.g,  'hAltm', NaN, 'CNEExt', NaN(3), 'hExt', NaN,...
            'vNExt', NaN(3,1), 'CBLExt', NaN(3), 'qBLExt', NaN(1, 4), ...
            'beta_m_ext', NaN(3, 1), 'DvScul_m_ext', NaN(3, 1), 'DrScrl_m_ext', NaN(3, 1));
        mid_t = double(i)*par_SINI.Tl;
        [tmp.out, tmp.auxOut] = sinistep_pgs(int8(0), in_SINI, cfg_SINI, par_SINI, iniCon_SINI, cst_SINI);
        out_RSINI.t(i+1,:) = mid_t;
        out_RSINI.cnt(i+1, :) = [tmp.out.cycCnt, tmp.out.HSCnt, tmp.out.NSCnt, tmp.out.LSCnt];
        out_RSINI.CNE(:, :, i+1) = tmp.out.CNE;
        out_RSINI.pos(i+1, :) = [tmp.out.L, tmp.out.lambda, tmp.out.h];
        out_RSINI.vN(i+1, :) = -tmp.out.vN';
        out_RSINI.vG(i+1, :) = -tmp.out.vG';
        out_RSINI.CBL(:, :, i+1) = tmp.out.CBL;
        out_RSINI.CBG(:, :, i+1) = tmp.out.CBG;
        out_RSINI.qBL(i+1, :) = tmp.out.qBL;
        out_RSINI.att(i+1, :) = [tmp.out.psi, tmp.out.theta, tmp.out.phi];
        out_RSINI.psiTrue(i+1) = tmp.out.psiTrue;
        out_RSINI.alpha(i+1) = tmp.out.alpha;
        out_RSINI.gN(i+1, :) = tmp.out.gN';
        out_RSINI.FCN(:, :, i+1) = tmp.out.FCN;
        auxOut_RSINI.altErr(i+1) = tmp.auxOut.delh_n;
        auxOut_RSINI.altErrIntGain(i+1) = tmp.auxOut.eVC3_n;
        auxOut_RSINI.coning(i+1, :) = tmp.auxOut.beta_l';
        auxOut_RSINI.sculling(i+1, :) = tmp.auxOut.DvScul_l';
        auxOut_RSINI.scrolling(i+1, :) = tmp.auxOut.DrScrl_l';
    end
    in_SINI = struct('specVelInc', in.accOut(i, :)', 'angleInc', -in.gyroOut(i, :)',...
        'g', par.g,  'hAltm', NaN, 'CNEExt', NaN(3), 'hExt', NaN,...
        'vNExt', NaN(3,1), 'CBLExt', NaN(3), 'qBLExt', NaN(1, 4), ...
        'beta_m_ext', NaN(3, 1), 'DvScul_m_ext', NaN(3, 1), 'DrScrl_m_ext', NaN(3, 1));
    mid_t = double(i-1)*par_SINI.Tl;
    [tmp.out, tmp.auxOut] = sinistep_pgs(int8(1), in_SINI, cfg_SINI, par_SINI, iniCon_SINI, cst_SINI);
    out_RSINI.t(i,:) = mid_t;
    out_RSINI.cnt(i, :) = [tmp.out.cycCnt, tmp.out.HSCnt, tmp.out.NSCnt, tmp.out.LSCnt];
    out_RSINI.CNE(:, :, i) = tmp.out.CNE;
    out_RSINI.pos(i, :) = [tmp.out.L, tmp.out.lambda, tmp.out.h];
    out_RSINI.vN(i, :) = -tmp.out.vN';
    out_RSINI.vG(i, :) = -tmp.out.vG';
    out_RSINI.CBL(:, :, i) = tmp.out.CBL;
    out_RSINI.CBG(:, :, i) = tmp.out.CBG;
    out_RSINI.qBL(i, :) = tmp.out.qBL;
    out_RSINI.att(i, :) = [tmp.out.psi, tmp.out.theta, tmp.out.phi];
    out_RSINI.psiTrue(i) = tmp.out.psiTrue;
    out_RSINI.alpha(i) = tmp.out.alpha;
    out_RSINI.gN(i, :) = tmp.out.gN';
    out_RSINI.FCN(:, :, i) = tmp.out.FCN;
    auxOut_RSINI.altErr(i) = tmp.auxOut.delh_n;
    auxOut_RSINI.altErrIntGain(i) = tmp.auxOut.eVC3_n;
    auxOut_RSINI.coning(i, :) = tmp.auxOut.beta_l';
    auxOut_RSINI.sculling(i, :) = tmp.auxOut.DvScul_l';
    auxOut_RSINI.scrolling(i, :) = tmp.auxOut.DrScrl_l';
end
close(hwb);
% 显示验证结果
disp('===================逆向导航===================');
out_sim = comparenavparam({out_RSINI}, ref, cst_SINI.RE, {16, 17, 18}, {'导航'}, '轨迹');
disp('最大位置误差（m）');
disp(max(abs(out_sim.dPos{1}), [], 1));
disp('最大速度误差（m/s）');
disp(max(abs(out_sim.dvG{1}), [], 1));
disp('最大姿态误差（″）');
disp(max(abs(out_sim.phi{1}), [], 1));
disp('最大正交误差（″）');
disp(max(abs(out_sim.dOrth{1}), [], 1));
disp('最大规范误差（ppm）');
disp(max(abs(out_sim.dNorm{1}), [], 1));
